#include <TL/robotManipulationDomain.h>


RobotManipulationSimulator sim;


void initSimulator(const char* configurationFile, bool takeMovie) {
	sim.shutdownAll();
	sim.loadConfiguration(configurationFile);
#ifdef MT_FREEGLUT
	orsDrawProxies = false;
	orsDrawJoints = false;
#endif
	sim.startOde();

	//sim.initWorldEditor();

	if (takeMovie)
		sim.startRevel();
	sim.startSwift();
	sim.simulate(50);
}


std::string removeBreaks(const std::string &str) {
	std::string result = str;
	while (int pos = result.find("\n") != std::string::npos)
		result = result.replace(pos, 1, "");
	return result;
}


void test(int argn,char** argv) {
	uint randSeed = 12345;
	rnd.seed(randSeed);

	// Set up logic
	TL::logicObjectManager::setPredicatesAndFunctions("language.dat");
	TL::logicObjectManager::writeLanguage("used_language.dat");

	MT::String sim_file("../../RMSim/configurations/situation.ors");
	initSimulator(sim_file, false);
	sim.simulate(50);

	// Get objects and set in logic database66
	uintA objs;
	sim.getObjects(objs);
	TL::logicObjectManager::setConstants(objs);

	uintA grabableObjects; grabableObjects.append(65); grabableObjects.append(66); grabableObjects.append(67); grabableObjects.append(68); grabableObjects.append(69);
	grabableObjects.append(76); grabableObjects.append(77); grabableObjects.append(78); grabableObjects.append(79);
	uintA putonTargetObjects; putonTargetObjects.append(65); putonTargetObjects.append(66); putonTargetObjects.append(67); putonTargetObjects.append(68); putonTargetObjects.append(69);
	putonTargetObjects.append(72); putonTargetObjects.append(72); putonTargetObjects.append(72); putonTargetObjects.append(72); putonTargetObjects.append(72); putonTargetObjects.append(72); 
	putonTargetObjects.append(73); putonTargetObjects.append(73); putonTargetObjects.append(73); putonTargetObjects.append(73); putonTargetObjects.append(73); putonTargetObjects.append(73); 

	// Sequence of actions to perform
	MT::String controlActions("grab(68) puton(66) grab(67) puton(65)");
	MT::String scaleActions1("grab(67) puton(73) grab(67) puton(73) grab(68) puton(72) grab(68) puton(69) grab(67) puton(72) grab(65) puton(73) grab(66) puton(73) grab(66) puton(73) grab(67) puton(69) grab(66) puton(67) grab(65) puton(67)");
	//scale actions2 = same actions, but with 72 und 73 swapped
	MT::String scaleActions2("grab(67) puton(72) grab(67) puton(72) grab(68) puton(73) grab(68) puton(69) grab(67) puton(73) grab(65) puton(72) grab(66) puton(72) grab(66) puton(72) grab(67) puton(73)");

	MT::String actionsStr = controlActions + " " + scaleActions1 + " " + scaleActions2;//("grab(68) puton(73) grab(65) puton(72)")
	/*const int NUM_ACTIONS = 100;
	bool grab = true;
	uint inhandObj = 0;
	MT::rnd.seed(12345);
	for (int i = 0; i < NUM_ACTIONS; i++) {
		if (i > 0) actionsStr << " ";
		if (grab) {
			inhandObj = MT::rnd.num(grabableObjects.N);
			actionsStr << "grab(" << grabableObjects(inhandObj) << ")";
		}
		else {
			uint obj;
			while ((obj = MT::rnd.num(putonTargetObjects.N)) == inhandObj) {}
			actionsStr << "puton(" << putonTargetObjects(obj) << ")";
		}
		grab = !grab;
	}*/
	cout<<"Actions: "<<actionsStr<<endl;

	AtomA actions;
	TL::logicObjectManager::getAtoms(actions, actionsStr);

	std::ofstream sampleFile("scaleSamples.dat");

	sampleFile << "[";

	uint i;
	FOR1D(objs, i) {
		sampleFile << objs(i);
		if (i != objs.N-1) sampleFile << " ";
	}

	sampleFile << "]" << std::endl;

	// Perform

	FOR1D(actions, i) {
		cout<<"TIME-STEP t="<<i<<endl;
		// OBSERVE STATE
		TL::State* s = TL::RobotManipulationDomain::observeLogic(&sim);

		sampleFile << std::endl;
		sampleFile << *s << std::endl;

		cout<<endl<<"OBSERVED STATE:"<<endl<<*s<<endl;
		//cout<<"Please press button to continue."<<endl;
		//sim.watch();
		// ACTION
		sampleFile << *actions(i) << std::endl;
		cout<<endl<<"ACTION #"<<i<<" "<<*actions(i)<<endl;
		TL::RobotManipulationDomain::performAction(actions(i), &sim, 250);
		cout<<"----------------------"<<endl;
	}

	// OBSERVE STATE
	TL::State* s = TL::RobotManipulationDomain::observeLogic(&sim);

	cout<<endl<<"OBSERVED STATE:"<<endl<<*s<<endl;

	sampleFile << std::endl;
	sampleFile << *s << std::endl;

	sampleFile.close();

	cout<<"Please press button to continue."<<endl;
	sim.watch();

	sim.shutdownAll();
}


int main(int argc, char** argv){
	test(argc, argv);
	return 0;
}
